#include <stdio.h>
#include <string.h>
#include <math.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "radar.h"
#include "main.h"
#include "lamp_ctrl.h"
#include "ws2812.h"
#include "uart_events.h"
// #include "nvs_value.h"
#include "h03_config.h"

static const char *TAG = "[RADAR]";

typedef struct{
    uint16_t    head;
    uint8_t     len_l;
    uint8_t     len_h;
    uint8_t     fun;
    uint8_t     addr1;
    uint8_t     addr2;
    uint8_t     data[0];
}radar_frame_t;

typedef struct {
    int motion_70cm;
    int motion_140cm;
    int motion_210cm;
    int static_70cm;
    int static_140cm;
    int static_210cm;
    int64_t timestamp;
} radar_data_t;

typedef struct {
    float current_focus;          // 当前专注度(0-100)
    float smoothed_focus;         // 平滑后的专注度
    float motion_variance;        // 动作波动方差
    float energy_change_rate;     // 能量变化率
    int motion_frequency;         // 动作频繁度
    int64_t last_update_time;     // 上次更新时间
    float focus_history[10];      // 专注度历史记录
    int history_index;            // 历史索引
} focus_data_t;

static QueueHandle_t radar_data_queue;
static focus_data_t focus_info;
static SemaphoreHandle_t state_mutex;
static SemaphoreHandle_t focus_mutex;

human_state_t human_state;
human_state_t human_state_ex;

learning_duration_t learning_duration;
uint8_t radar_state = 0;

bool radar_ota_ret;
extern bool radar_ota_flag;
extern uint32_t binary_file_length;
extern bool time_sync_updata_flag;

uint8_t present_state;
extern uint32_t colors[9];
extern uint32_t led_color;
extern uint16_t blink_delay;
bool inseat_state;
bool comeandgo_updata_flag;

static char frame_head[4] = {0xfd, 0xfc, 0xfb, 0xfa};
static char frame_end[4] = {0x04, 0x03, 0x02, 0x01};

void radar_set_en(void)
{
    char data[14] = {0xfd, 0xfc, 0xfb, 0xfa, 0x04, 0x00, 0xff, 0x00, 0x02, 0x00, 0x04, 0x03, 0x02, 0x01};

    uart_write_bytes(RADAR_UART_NUM, data, 14);
    radar_state = 1;
    // printf_buf("<-", (uint8_t *)data, 14);
}

void radar_set_mode(uint8_t mode)
{
    char data[18] = {0xfd, 0xfc, 0xfb, 0xfa, 0x08, 0x00, 0x12, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x04, 0x03, 0x02, 0x01};

    data[10] = mode;
    uart_write_bytes(RADAR_UART_NUM, data, 18);
    // printf_buf("<-", (uint8_t *)data, 18);
}

void radar_read_info(uint16_t cmd)
{
    char data[12] = {0};

    memcpy(&data[0], &frame_head, 4);

    data[4] = 0x02;
    data[5] = 0x00;
    data[6] = cmd & 0xff;
    data[7] = (cmd >> 8) & 0xff;
    memcpy(&data[8], &frame_end, 4);

    uart_write_bytes(RADAR_UART_NUM, data, 12);
    // printf_buf("<-", (uint8_t *)data, 12);
}

void radar_set_param(uint16_t address, uint32_t value)
{
    char data[18] = {0};

    memcpy(&data[0], &frame_head, 4);
    data[4] = 0x08;
    data[5] = 0x00;
    data[6] = 0x07;
    data[7] = 0x00;
    data[8] = address & 0xff;
    data[9] = (address >> 8) & 0xff;
    data[10] = value & 0xff;
    data[11] = (value >> 8) & 0xff;
    data[12] = (value >> 16) & 0xff;
    data[13] = (value >> 24) & 0xff;
    memcpy(&data[14], &frame_end, 4);

    uart_write_bytes(RADAR_UART_NUM, data, 18);
    // printf_buf("<-", (uint8_t *)data, 18);
}

void radar_set_threshold(uint16_t address1, uint32_t threshold1, uint16_t address2, uint32_t threshold2, uint16_t address3, uint32_t threshold3, uint16_t address4, uint32_t threshold4)
{
    char data[36] = {0};

    memcpy(&data[0], &frame_head, 4);
    data[4] = 0x1a;
    data[5] = 0x00;
    data[6] = 0x07;
    data[7] = 0x00;
    data[8] = address1 & 0xff;
    data[9] = (address1 >> 8) & 0xff;
    data[10] = threshold1 & 0xff;
    data[11] = (threshold1 >> 8) & 0xff;
    data[12] = (threshold1 >> 16) & 0xff;
    data[13] = (threshold1 >> 24) & 0xff;
    data[14] = address2 & 0xff;
    data[15] = (address2 >> 8) & 0xff;
    data[16] = threshold2 & 0xff;
    data[17] = (threshold2 >> 8) & 0xff;
    data[18] = (threshold2 >> 16) & 0xff;
    data[19] = (threshold2 >> 24) & 0xff;
    data[20] = address3 & 0xff;
    data[21] = (address3 >> 8) & 0xff;
    data[22] = threshold3 & 0xff;
    data[23] = (threshold3 >> 8) & 0xff;
    data[24] = (threshold3 >> 16) & 0xff;
    data[25] = (threshold3 >> 24) & 0xff;
    data[26] = address4 & 0xff;
    data[27] = (address4 >> 8) & 0xff;
    data[28] = threshold4 & 0xff;
    data[29] = (threshold4 >> 8) & 0xff;
    data[30] = (threshold4 >> 16) & 0xff;
    data[31] = (threshold4 >> 24) & 0xff;
    memcpy(&data[32], &frame_end, 4);

    uart_write_bytes(RADAR_UART_NUM, data, 36);
    // printf_buf("<-", (uint8_t *)data, 36);
}

void radar_read_param(uint16_t address)
{
    char data[14] = {0};

    memcpy(&data[0], &frame_head, 4);
    data[4] = 0x04;
    data[5] = 0x00;
    data[6] = 0x08;
    data[7] = 0x00;    
    data[8] = address | 0xff;
    data[9] = (address >> 8) | 0xff;
    
    uart_write_bytes(RADAR_UART_NUM, data, 14);
    // printf_buf("<-", (uint8_t *)data, 14);
}

void radar_set_end(void)
{
    char data[12] = {0xfd, 0xfc, 0xfb, 0xfa, 0x02, 0x00, 0xfe, 0x00, 0x04, 0x03, 0x02, 0x01};
    uart_write_bytes(RADAR_UART_NUM, data, 12);
    // printf_buf("<-", (uint8_t *)data, 12);
}

uint8_t radar_cal_db(uint32_t data)
{
    if(data == 0){
        return 0;
    }else{
        return log10(data)*10;
    }
}

#define WINDOW_SIZE 10

#define ENERGY_TOTAL_THRESHOLD_NOBODY   180    // 总能量阈值
#define ENERGY_MAX_THRESHOLD_NOBODY     40     // 最大能量阈值（单独设置）
#define MOTION_RATIO_THRESHOLD       0.3     // 动作能量占比阈值
#define ENERGY_VARIANCE_THRESHOLD   50.0     // 能量方差阈值
#define ENERGY_ENTROPY_THRESHOLD     1.5     // 能量熵阈值

#define FOCUS_SMOOTHING_FACTOR   0.1f    // 专注度平滑系数
#define FOCUS_UPDATE_INTERVAL    1000    // 专注度更新间隔(ms)
#define MOTION_VARIANCE_MAX      100.0f  // 动作方差最大值
#define ENERGY_CHANGE_MAX        50.0f   // 能量变化最大值

static radar_data_t data_window[WINDOW_SIZE];
static int window_index = 0;
static int window_count = 0;

static int calculate_total_energy(const radar_data_t *data)
{
    return data->motion_70cm + data->motion_140cm + data->motion_210cm +
           data->static_70cm + data->static_140cm + data->static_210cm;
}

static int find_max_energy(const radar_data_t *data)
{
    int max_energy = data->motion_70cm;
    int energies[6] = {
        data->motion_70cm, data->motion_140cm, data->motion_210cm,
        data->static_70cm, data->static_140cm, data->static_210cm
    };
    
    for (int i = 1; i < 6; i++) {
        if (energies[i] > max_energy) {
            max_energy = energies[i];
        }
    }
    
    return max_energy;
}

static float calculate_motion_ratio(const radar_data_t *data)
{
    int motion_total = data->motion_70cm + data->motion_140cm + data->motion_210cm;
    int static_total = data->static_70cm + data->static_140cm + data->static_210cm;
    int total_energy = motion_total + static_total;
    
    if (total_energy == 0) return 0.0f;
    return (float)motion_total / total_energy;
}

static float calculate_energy_entropy(const radar_data_t *data)
{
    int energies[6] = {
        data->motion_70cm, data->motion_140cm, data->motion_210cm,
        data->static_70cm, data->static_140cm, data->static_210cm
    };
    
    int total = 0;
    for (int i = 0; i < 6; i++) {
        total += energies[i];
    }
    
    if (total == 0) return 0.0f;
    
    float entropy = 0.0f;
    for (int i = 0; i < 6; i++) {
        float p = (float)energies[i] / total;
        if (p > 0.0f) {
            entropy -= p * logf(p);
        }
    }
    
    return entropy;
}

static float calculate_energy_variance(void)
{
    if (window_count < 2) return 0.0f;
    
    // 计算窗口内总能量的均值
    float mean = 0.0f;
    for (int i = 0; i < window_count; i++) {
        mean += calculate_total_energy(&data_window[i]);
    }
    mean /= window_count;
    
    // 计算方差
    float variance = 0.0f;
    for (int i = 0; i < window_count; i++) {
        float diff = calculate_total_energy(&data_window[i]) - mean;
        variance += diff * diff;
    }
    variance /= (window_count - 1);
    
    return variance;
}

static float calculate_motion_variance(void)
{
    if (window_count < 2) return 0.0f;
    
    // 计算动作能量的均值
    float mean = 0.0f;
    for (int i = 0; i < window_count; i++) {
        mean += data_window[i].motion_70cm + data_window[i].motion_140cm + data_window[i].motion_210cm;
    }
    mean /= window_count;
    
    // 计算方差
    float variance = 0.0f;
    for (int i = 0; i < window_count; i++) {
        float motion_energy = data_window[i].motion_70cm + data_window[i].motion_140cm + data_window[i].motion_210cm;
        float diff = motion_energy - mean;
        variance += diff * diff;
    }
    variance /= (window_count - 1);
    
    return variance;
}

static float calculate_energy_change_rate(const radar_data_t *current, const radar_data_t *prev)
{
    int current_energy = calculate_total_energy(current);
    int prev_energy = calculate_total_energy(prev);
    
    if (prev_energy == 0) return 0.0f;
    return fabsf((float)(current_energy - prev_energy) / prev_energy);
}

static int calculate_motion_frequency(const radar_data_t *current_data)
{
    static int state_change_count = 0;
    static float last_motion_ratio = 0.0f;
    static int64_t last_check_time = 0;
    
    float current_motion_ratio = calculate_motion_ratio(current_data);
    int64_t current_time = esp_timer_get_time();
    
    // 每500ms检查一次状态变化
    if (last_check_time == 0 || (current_time - last_check_time) > 500000) {
        // 如果动作占比变化超过阈值，认为是状态变化
        if (fabsf(current_motion_ratio - last_motion_ratio) > 0.15f) {
            state_change_count++;
        }
        
        last_motion_ratio = current_motion_ratio;
        last_check_time = current_time;
    }
    
    // 每5秒重置计数
    if (last_check_time > 0 && (current_time - last_check_time) > 5000000) {
        state_change_count = 0;
    }
    
    return state_change_count;
}

static float calculate_focus_score(const radar_data_t *current_data, const radar_data_t *prev_data)
{
    if (prev_data == NULL) return 80.0f; // 初始专注度
    
    // 1. 计算动作能量变化率
    int current_motion = current_data->motion_70cm + current_data->motion_140cm + current_data->motion_210cm;
    int prev_motion = prev_data->motion_70cm + prev_data->motion_140cm + prev_data->motion_210cm;
    float motion_change_rate = fabsf((float)(current_motion - prev_motion) / (prev_motion + 1));
    
    // 2. 计算总能量变化率
    int current_total = calculate_total_energy(current_data);
    int prev_total = calculate_total_energy(prev_data);
    float energy_change_rate = fabsf((float)(current_total - prev_total) / (prev_total + 1));
    
    // 3. 计算动作波动方差（基于滑动窗口）
    float motion_variance = calculate_motion_variance();
    
    // 4. 计算动作频繁度（基于状态变化次数）
    int motion_frequency = calculate_motion_frequency(current_data);
    
    // 5. 综合计算专注度分数（0-100）
    float focus_score = 100.0f;
    
    // 动作变化率惩罚（0-30分）
    float change_penalty = motion_change_rate * 30.0f;
    focus_score -= fminf(change_penalty, 30.0f);
    
    // 能量变化率惩罚（0-25分）
    float energy_penalty = energy_change_rate * 25.0f;
    focus_score -= fminf(energy_penalty, 25.0f);
    
    // 动作方差惩罚（0-25分）
    float variance_penalty = (motion_variance / MOTION_VARIANCE_MAX) * 25.0f;
    focus_score -= fminf(variance_penalty, 25.0f);
    
    // 动作频繁度惩罚（0-20分）
    float frequency_penalty = ((float)motion_frequency / 10.0f) * 20.0f;
    focus_score -= fminf(frequency_penalty, 20.0f);
    
    // 确保在0-100范围内
    return fmaxf(0.0f, fminf(100.0f, focus_score));
}

static void update_focus_level(const radar_data_t *current_data)
{
    static radar_data_t prev_data = {0};
    static int first_run = 1;
        
    int64_t current_time = esp_timer_get_time();
    
    // 每秒更新一次专注度
    if (first_run || (current_time - focus_info.last_update_time) > FOCUS_UPDATE_INTERVAL * 1000) {
        
        if (!first_run) {
            // 计算新的专注度
            float new_focus = calculate_focus_score(current_data, &prev_data);
            
            // 平滑处理（避免剧烈变化）
            focus_info.smoothed_focus = focus_info.smoothed_focus * (1.0f - FOCUS_SMOOTHING_FACTOR) + 
                                      new_focus * FOCUS_SMOOTHING_FACTOR;
            
            focus_info.current_focus = focus_info.smoothed_focus;
            
            // 更新历史记录
            focus_info.focus_history[focus_info.history_index] = focus_info.current_focus;
            focus_info.history_index = (focus_info.history_index + 1) % 10;
            
            // 记录其他指标
            focus_info.motion_variance = calculate_motion_variance();
            focus_info.energy_change_rate = calculate_energy_change_rate(current_data, &prev_data);
            focus_info.motion_frequency = calculate_motion_frequency(current_data);
        }
        
        prev_data = *current_data;
        focus_info.last_update_time = current_time;
        first_run = 0;
    }
}

float get_current_focus_level(void)
{
    float focus;
    xSemaphoreTake(focus_mutex, portMAX_DELAY);
    focus = focus_info.current_focus;
    xSemaphoreGive(focus_mutex);
    return focus;
}

static void state_detection_task(void *arg)
{
    radar_data_t radar_data;
    uint32_t flow_color = 0;
    static uint32_t flow_color_ex = 0;
    static uint8_t flow_state_level_ex = 0;
    static uint8_t comeandgo_times;
    static uint16_t comeandgo_taketime[4];

    while (1) {
        if (xQueueReceive(radar_data_queue, &radar_data, portMAX_DELAY) == pdTRUE) {
            
            // 更新滑动窗口
            data_window[window_index] = radar_data;
            window_index = (window_index + 1) % WINDOW_SIZE;
            if (window_count < WINDOW_SIZE){
                window_count++;
            } 
            
            // 提取特征
            int total_energy = calculate_total_energy(&radar_data);
            int max_energy = find_max_energy(&radar_data);
            float motion_ratio = calculate_motion_ratio(&radar_data);
            float energy_entropy = calculate_energy_entropy(&radar_data);
            float energy_variance = calculate_energy_variance();

            
            // 状态判断逻辑
            float current_focus = 0.0f;
            
            if (total_energy < ENERGY_TOTAL_THRESHOLD_NOBODY && max_energy < ENERGY_MAX_THRESHOLD_NOBODY) {
                human_state.body_status = 0;
                human_state.activity_freq = 0;
                current_focus = 0.0f;
            } else {
                // 更新专注度（仅在有人状态下）
                human_state.body_status = 0;
                update_focus_level(&radar_data);
                current_focus = get_current_focus_level();
                
                if (motion_ratio < MOTION_RATIO_THRESHOLD && energy_entropy < ENERGY_ENTROPY_THRESHOLD && energy_variance < ENERGY_VARIANCE_THRESHOLD) {
                    human_state.activity_freq = 2;
                } else {
                    human_state.activity_freq = 1;
                    current_focus = 0.0f; // 非学习状态专注度为0
                }
            }
            human_state.flow_state = current_focus;

            printf("%d %d %d %d %d %d | tt[%d] me[%d] mr[%0.3f] ee[%0.3f] ev[%0.3f] cf[%0.3f]\n", radar_data.motion_70cm,
                                            radar_data.motion_140cm,
                                            radar_data.motion_210cm,
                                            radar_data.static_70cm,
                                            radar_data.static_140cm,
                                            radar_data.static_210cm,
                                            total_energy,
                                            max_energy,
                                            motion_ratio,
                                            energy_entropy,
                                            energy_variance,
                                            current_focus
                                        );

            if(human_state.body_status != human_state_ex.body_status){
                human_state_ex.body_status = human_state.body_status;
                event_flag.body_status = 1;
                if(human_state.body_status == 1){
                    inseat_state = 1;
                }else{
                    inseat_state = 0;
                    comeandgo_taketime[comeandgo_times%4] = timer_count.comeandgo_count;
                    timer_count.comeandgo_count = 0;
                    uint16_t comeandgo_sum = 0;
                    for(uint8_t t=0; t<4; t++){
                        comeandgo_sum += comeandgo_taketime[t];
                    }
                    if(comeandgo_sum < 300 && comeandgo_times >= 4){
                        comeandgo_updata_flag = 1;
                        comeandgo_times = 0;
                    }else{
                        comeandgo_times++;
                    }
                }
                // cJSON_creat_event("body_status", human_state.body_status);
            }
            if(human_state.flow_state != human_state_ex.flow_state){
                human_state_ex.flow_state = human_state.flow_state;
                if(human_state.flow_state > 100){
                    human_state.flow_state_level = human_state.flow_state;
                }else if(human_state.flow_state > 97){
                    human_state.flow_state_level = 10;
                }else if(human_state.flow_state > 85){
                    human_state.flow_state_level = 9;
                }else if(human_state.flow_state > 75){
                    human_state.flow_state_level = 8;
                }else if(human_state.flow_state > 69){
                    human_state.flow_state_level = 7;
                }else if(human_state.flow_state > 62){
                    human_state.flow_state_level = 6;
                }else if(human_state.flow_state > 54){
                    human_state.flow_state_level = 5;
                }else if(human_state.flow_state > 39){
                    human_state.flow_state_level = 4;
                }else if(human_state.flow_state > 29){
                    human_state.flow_state_level = 3;
                }else if(human_state.flow_state > 19){
                    human_state.flow_state_level = 2;
                }else if(human_state.flow_state > 0){
                    human_state.flow_state_level = 1;
                }else{
                    human_state.flow_state_level = 0;
                }
                if(human_state.flow_state_level != flow_state_level_ex){
                    flow_state_level_ex = human_state.flow_state_level;
                    event_flag.flow_state = 1;
                    if(present_state == 2){
                        if(human_state.flow_state_level > 7){
                            flow_color = colors[4];
                        }else if(human_state.flow_state_level > 4){
                            flow_color = colors[8];
                        }else if(human_state.flow_state_level > 0){
                            flow_color = colors[2];
                        }
                        if(flow_color != flow_color_ex){
                            flow_color_ex = flow_color;
                            if(top_led_mode == 2){
                                led_color = flow_color;
                                blink_delay = 1600;
                                led_notify = FADE_NOTIFICATION;
                                event_flag.led_break = 1;
                            }
                        }
                    }
                }
                // cJSON_creat_event("flow_state", human_state.flow_state);
            }
            if(human_state.position_interval != human_state_ex.position_interval){
                human_state_ex.position_interval = human_state.position_interval;
                event_flag.position_interval = 1;
                // cJSON_creat_event("body_status", human_state.body_status);
            }


        }
    }
}

void detect_data_parse(uint8_t *buf, uint16_t length)
{
    uint32_t lg_db = 0;
    uint8_t body_db[32] = {0};
    static uint32_t radar_count = 0;

    // human_state.body_status = buf[0];
    human_state.body_distance = buf[1] | (buf[2] << 8);
    for(uint8_t i = 0; i < 32; i++){
        lg_db = buf[3 + i*4] | (buf[4 + i*4] << 8) | (buf[5 + i*4] << 16) | (buf[6 + i*4] << 24);
        body_db[i] = radar_cal_db(lg_db);
    }
    radar_data_t radar_data;
    radar_data.motion_70cm = body_db[0];
    radar_data.motion_140cm = body_db[1];
    radar_data.motion_210cm = body_db[2];
    radar_data.static_70cm = body_db[16];
    radar_data.static_140cm = body_db[17];
    radar_data.static_210cm = body_db[18];

    if (xQueueSend(radar_data_queue, &radar_data, 0) != pdTRUE) {
        ESP_LOGE(TAG, "Radar data queue full");
    }

    // printf("%d %d %d %d %d %d\n", body_db[0], body_db[1], body_db[2], body_db[16], body_db[17], body_db[18]);
    radar_count++;

#if 0
    if(human_state.body_status == 1){
        body_mov_base = radar_cal_single_adv(body_sli1_data, RADAR_BUF_SIZE);
        // body_mov_base = radar_cal_adv(human_state.body_movement, radar_count);

        uint8_t tmp_data = 0;
        if(human_state.body_movement >= body_mov_base){
            tmp_data = (human_state.body_movement - body_mov_base)*100*(1-RISING_SPEED)*CHANGE_SPEED;
            if(flow_state_tmp > tmp_data){
                flow_state_tmp = flow_state_tmp - tmp_data;
            }else{
                flow_state_tmp = 0;
            }
        }else{
            tmp_data = (body_mov_base - human_state.body_movement)*100*RISING_SPEED*CHANGE_SPEED;
            if(flow_state_tmp + tmp_data < 10000){
                flow_state_tmp = flow_state_tmp + tmp_data;
            }else{
                flow_state_tmp = 10000;
            }
        }
        human_state.flow_state = (uint8_t)(flow_state_tmp/100);
        human_state.position_interval = 5;
        radar_count++;
        if(radar_count >= 523636){
            radar_count = 0;
        }
    }else{
        human_state.flow_state = 0;
        flow_state_tmp = 0;
        human_state.position_interval = 0;
        radar_count = 0;
    }

    // double base_tmp = (double)body_mov_base/100;
    double flow_tmp = (double)flow_state_tmp/100;
    // printf("%d | %ld %d | %0.2f %d\n", someone_count, body_mov_base, human_state.body_movement, flow_tmp, human_state.flow_state);
    
    if(human_state.body_status != human_state_ex.body_status){
        human_state_ex.body_status = human_state.body_status;
        event_flag.body_status = 1;
        if(human_state.body_status == 1){
            inseat_state = 1;
        }else{
            inseat_state = 0;
            comeandgo_taketime[comeandgo_times%4] = timer_count.comeandgo_count;
            timer_count.comeandgo_count = 0;
            uint16_t comeandgo_sum = 0;
            for(uint8_t t=0; t<4; t++){
                comeandgo_sum += comeandgo_taketime[t];
            }
            if(comeandgo_sum < 300 && comeandgo_times >= 4){
                comeandgo_updata_flag = 1;
                comeandgo_times = 0;
            }else{
                comeandgo_times++;
            }
            radar_count = 0;
        }
        // cJSON_creat_event("body_status", human_state.body_status);
    }
    if(human_state.flow_state != human_state_ex.flow_state){
        human_state_ex.flow_state = human_state.flow_state;
        if(human_state.flow_state > 100){
            human_state.flow_state_level = human_state.flow_state;
        }else if(human_state.flow_state > 97){
            human_state.flow_state_level = 10;
        }else if(human_state.flow_state > 85){
            human_state.flow_state_level = 9;
        }else if(human_state.flow_state > 75){
            human_state.flow_state_level = 8;
        }else if(human_state.flow_state > 69){
            human_state.flow_state_level = 7;
        }else if(human_state.flow_state > 62){
            human_state.flow_state_level = 6;
        }else if(human_state.flow_state > 54){
            human_state.flow_state_level = 5;
        }else if(human_state.flow_state > 39){
            human_state.flow_state_level = 4;
        }else if(human_state.flow_state > 29){
            human_state.flow_state_level = 3;
        }else if(human_state.flow_state > 19){
            human_state.flow_state_level = 2;
        }else if(human_state.flow_state > 0){
            human_state.flow_state_level = 1;
        }else{
            human_state.flow_state_level = 0;
        }
        if(human_state.flow_state_level != flow_state_level_ex){
            flow_state_level_ex = human_state.flow_state_level;
            event_flag.flow_state = 1;
            if(present_state == 2){
                if(human_state.flow_state_level > 7){
                    flow_color = colors[4];
                }else if(human_state.flow_state_level > 4){
                    flow_color = colors[8];
                }else if(human_state.flow_state_level > 0){
                    flow_color = colors[2];
                }
                if(flow_color != flow_color_ex){
                    flow_color_ex = flow_color;
                    if(top_led_mode == 2){
                        led_color = flow_color;
                        blink_delay = 1600;
                        led_notify = FADE_NOTIFICATION;
                        event_flag.led_break = 1;
                    }
                }
            }
        }
        // cJSON_creat_event("flow_state", human_state.flow_state);
    }
    if(human_state.position_interval != human_state_ex.position_interval){
        human_state_ex.position_interval = human_state.position_interval;
        event_flag.position_interval = 1;
        // cJSON_creat_event("body_status", human_state.body_status);
    }
#endif
}
uint8_t *detect_data = NULL;
uint8_t radar_cmd_NO = 0;

void radar_data_parse(char *buf, uint16_t length)
{
    static uint8_t frame_len = 0;
    uint8_t cmd = 0;
    uint16_t tmp_buf = 0;
    uint32_t tmp_buf32 = 0;
    char radar_verson1[16];
    char radar_sn[16];
    static bool collect_need = false;
    static uint8_t detect_data_pos = 0;
    static uint8_t count = 0;

    if(collect_need == 1){
        if(buf[length-4] == 0xf8 && buf[length-1] == 0xf5){
            collect_need = 0;
            memcpy(detect_data + detect_data_pos, buf, length-4);
            detect_data_parse(detect_data, frame_len);
        } 
    }else if(buf[length-2] == 0x0d && buf[length-1] == 0x0a){
        if(strstr(buf, "OFF") != NULL){
            human_state.body_status = 0;
        }else if(strstr(buf, "distance") != NULL){
            human_state.body_status = 1;
            char *distance = strstr(buf, ":") + 1;
            char *distance_end = strstr(distance, "\r");
            *(distance_end) = '\0';
            human_state.body_distance = atoi(distance);
        }
        count++;
        if(count == 100){
            count = 0;
            radar_set_en();
        }
    }else{
        if(buf[0] == 0xfd && buf[length-4] == 0x04){
            cmd_frame_t *cmd_frame = NULL;
            cmd_frame = (cmd_frame_t *)buf;
            frame_len = cmd_frame->len_l | (cmd_frame->len_h << 8);
            printf("frame_len: %d\n", frame_len);
            if(frame_len == length - 10){
                cmd = cmd_frame->cmd_l;
                // printf("cmd: %02x\n", cmd);
                switch(cmd){
                    case 0xff:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                tmp_buf = cmd_frame->data[0] | (cmd_frame->data[1] << 8);
                                printf("protocol_verson: %04x\n", tmp_buf);
                                tmp_buf = cmd_frame->data[2] | (cmd_frame->data[3] << 8);
                                printf("buffer_size: 0x%04x\n", tmp_buf);
                                if(radar_state == 1){
                                    radar_state = 2;
                                    radar_read_info(0x0000);
                                }
                            }
                        }
                        break;
                    case 0x00:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                tmp_buf = cmd_frame->data[0] | (cmd_frame->data[1] << 8);
                                memcpy(radar_verson1, cmd_frame->data + 2, tmp_buf);
                                radar_verson1[tmp_buf] = '\0';
                                printf("radar_verson: %s\n", radar_verson1);
                                radar_state = 3;
                                radar_read_info(0x0011);
                            }
                        }
                        break;
                    case 0x11:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                tmp_buf = cmd_frame->data[0] | (cmd_frame->data[1] << 8);
                                memcpy(radar_sn, cmd_frame->data + 2, tmp_buf);
                                radar_sn[tmp_buf] = '\0';
                                printf("radar_sn: %s\n", radar_sn);
                                radar_state = 4;
                                radar_set_param(0x0001, 20);
                                radar_cmd_NO = 1;
                            }
                        }
                        break;
                    case 0x07:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                printf("set param OK\n");
                                vTaskDelay(100 / portTICK_PERIOD_MS);
                                if(radar_state == 4){
                                    radar_state = 5;
                                    radar_set_param(0x0004, 20);
                                    radar_cmd_NO = 2;
                                }else if(radar_state == 5){
                                    radar_state = 6;
                                    radar_set_threshold(0x0010, 0x000000c8,
                                        0x0011, 0x000000c8,
                                        0x0012, 0x00000b00,
                                        0x0013, 0x00000596);
                                }else if(radar_state == 6){
                                    radar_state = 7;
                                    radar_set_threshold(0x0014, 0x00000596,
                                        0x0015, 0x00000596,
                                        0x0016, 0x00000596,
                                        0x0017, 0x00000596);
                                }else if(radar_state == 7){
                                    radar_state = 8;
                                    radar_set_threshold(0x0018, 0x00000596,
                                        0x0019, 0x00000596,
                                        0x001a, 0x00000596,
                                        0x001b, 0x00000596);
                                }else if(radar_state == 8){
                                    radar_state = 9;
                                    radar_set_threshold(0x001c, 0x00000596,
                                        0x001d, 0x00000596,
                                        0x001e, 0x00000596,
                                        0x001f, 0x00000596);
                                }else if(radar_state == 9){
                                    radar_state = 10;
                                    radar_set_threshold(0x0030, 0x000000c8,
                                        0x0031, 0x000000c8,
                                        0x0032, 0x00000eae,
                                        0x0033, 0x000005c4);
                                }else if(radar_state == 10){
                                    radar_state = 11;
                                    radar_set_threshold(0x0034, 0x000005c4,
                                        0x0035, 0x000005c4,
                                        0x0036, 0x000005c4,
                                        0x0037, 0x000005c4);
                                }else if(radar_state == 11){
                                    radar_state = 12;
                                    radar_set_threshold(0x0038, 0x000005c4,
                                        0x0039, 0x000005c4, 
                                        0x003a, 0x000005c4,
                                        0x003b, 0x000005c4);
                                }else if(radar_state == 12){
                                    radar_state = 13;
                                    radar_set_threshold(0x003c, 0x000005c4,
                                        0x003d, 0x000005c4,
                                        0x003e, 0x000005c4,
                                        0x003f, 0x000005c4);
                                }else if(radar_state == 13){
                                    radar_state = 14;
                                    radar_set_mode(0x04);
                                }
                            }else{
                                printf("set param fail\n");
                            }
                        }
                        break;
                    case 0x08:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                tmp_buf32 = cmd_frame->data[0] | (cmd_frame->data[1] << 8) | (cmd_frame->data[2] << 16) | (cmd_frame->data[3] << 24);
                                printf("get_param: %ld\n", tmp_buf32);
                            }
                        }
                        break;
                    case 0x12:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                printf("set_mode OK\n");
                                radar_state = 14;
                                radar_set_end();
                            }
                        }
                        break;
                    case 0xfe:
                        if(cmd_frame->cmd_h == 0x01){
                            if(cmd_frame->ack == 0x00){
                                radar_state = 15;
                                printf("set end\n");
                            }
                        }
                        break;
                    default:
                        break;
                }
            }
        }else if(buf[0] == 0xf4 && buf[3] == 0xf1){
            cmd_frame_t *cmd_frame = NULL;
            cmd_frame = (cmd_frame_t *)buf;
            frame_len = cmd_frame->len_l | (cmd_frame->len_h << 8);
            // printf("frame_len: %d\n", frame_len);
            if(frame_len != length - 10){
                collect_need = 1;
                if(detect_data == NULL){
                    detect_data = (uint8_t *)malloc(frame_len);
                }
                memcpy(detect_data, buf+6, length - 6);
                detect_data_pos = length - 6;
            }else{
                memcpy(detect_data, buf+6, length - 10);
            }
        }
    }
}

void radar_init(void)
{
    ESP_LOGI(TAG, "Radar init");

    radar_data_queue = xQueueCreate(20, sizeof(radar_data_t));
    state_mutex = xSemaphoreCreateMutex();
    focus_mutex = xSemaphoreCreateMutex();

    memset(&focus_info, 0, sizeof(focus_info));
    focus_info.current_focus = 80.0f;  // 初始专注度
    focus_info.smoothed_focus = 80.0f;

    xTaskCreate(state_detection_task, "state_detector", 4096, NULL, 4, NULL);
}
